/*
 * Copyright (c) 2022 hpmicro
 *
 * SPDX-License-Identifier: BSD-3-Clause
 *
 */

#include <stdio.h>
#include <string.h>

#include "board.h"
#include "hpm_sysctl_drv.h"
#include "hpm_l1c_drv.h"
#include "hpm_debug_console.h"
#include "hpm_gpio_drv.h"
#include "hpm_uart_drv.h"
#include "sec_core_img.h"

static void secondary_core_image_load_and_run(void);

static bool get_cpu_halt_state(uint8_t cpuid)
{
    return HPM_SYSCTL->CPU[cpuid].LP & SYSCTL_CPU_LP_HALT_MASK;
}

#define LED_CTRL BOARD_R_GPIO_CTRL
#define LED_PORT BOARD_R_GPIO_INDEX
#define LED_PIN  BOARD_R_GPIO_PIN
#define LED_CHAR 'A'
#define LED_DELAY 1000

int main(void)
{
    int u;
    board_init();
    board_init_led_pins();

    printf("CPU HALT: %d %d\n", get_cpu_halt_state(0), get_cpu_halt_state(1));
    printf("hello world from primary core\n");

    secondary_core_image_load_and_run();

    while (1) {
        uart_send_byte(HPM_UART0, LED_CHAR);
        gpio_write_pin(LED_CTRL, LED_PORT, LED_PIN, BOARD_LED_ON_LEVEL);
        board_delay_ms(LED_DELAY);

        gpio_write_pin(LED_CTRL, LED_PORT, LED_PIN, BOARD_LED_OFF_LEVEL);
        board_delay_ms(LED_DELAY);
    }
    return 0;
}

static void secondary_core_image_load_and_run(void)
{
#ifdef SEC_CORE_IMG_START
    if (!sysctl_is_cpu1_released(HPM_SYSCTL)) {
        printf("\n\n");
        printf("Copying secondary core image to destination memory...\n");

        uint32_t sec_core_img_sys_addr = core_local_mem_to_sys_address(HPM_CORE1, (uint32_t)SEC_CORE_IMG_START);
        printf("sec_core_img_sys_addr = %p\n", sec_core_img_sys_addr);
        memcpy((void *)sec_core_img_sys_addr, sec_core_img, sec_core_img_size);
        uint32_t aligned_start = HPM_L1C_CACHELINE_ALIGN_DOWN(sec_core_img_sys_addr);
        uint32_t aligned_end = HPM_L1C_CACHELINE_ALIGN_UP(sec_core_img_sys_addr + sec_core_img_size);
        uint32_t aligned_size = aligned_end - aligned_start;
        l1c_dc_flush(aligned_start, aligned_size);
        printf("Starting secondary core...\n");
        printf("RGB leds should blinking...\n");
        sysctl_set_cpu1_entry(HPM_SYSCTL, (uint32_t)SEC_CORE_IMG_START);
        sysctl_release_cpu1(HPM_SYSCTL);
    }
#endif
}
